#include "mex.h"
#include "matrix.h"
#include <cstdio>
#include <cstdlib>
#include <vector> 
#include <time.h>
#include <errno.h>
#include "./source/pnmfile.h"
#include "./source/filter.h"
#include "./source/image.h"
#include "./source/misc.h"
#include "./source/matrix.h"

    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////

using namespace vlib;
using namespace std;

double large = 1E10F;



    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
    //////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////////
/*
 * Simple discontinuity costs based on local contrast
 */
void initializeW(image<uchar> *I, 
		 image<double> * &whorz,image<double> * &wvert) {  
  int width = I->width();
  int height = I->height();

  double aver = 0.0;
  int total = 0;
  for (int y = 0; y < height-1; y++) {
    for (int  x = 0; x < width-1; x++) {
      aver = aver + 
	abs(imRef(I,x,y)-imRef(I,x+1,y))+
	abs(imRef(I,x,y)-imRef(I,x,y+1));
      total = total+2;
    }
  }
  aver = aver/total;	

  whorz = new image<double>(width, height);
  wvert = new image<double>(width, height);

  for (int y = 0; y < height; y++) {
    for (int x = 0; x < width-1; x++) {
      double diff = abs(imRef(I,x+1,y)-imRef(I,x,y));
      if (diff > 3.0*aver) 
	imRef(whorz,x,y) = 1;
      else if (diff > 2.0*aver) 
	imRef(whorz,x,y) = 2;
      else if (diff > 1.5*aver)
	imRef(whorz,x,y) = 3;
      else imRef(whorz,x,y) = 8;
    }
  }

  for (int  y = 0; y < height-1; y++) {
    for (int x = 0; x < width; x++ ) {
      double diff = abs(imRef(I,x,y+1)-imRef(I,x,y));
      if (diff > 3.0*aver) 
	imRef(wvert,x,y) = 1;
      else if (diff > 2.0*aver) 
	imRef(wvert,x,y) = 2;
      else if (diff > 1.5*aver) 
	imRef(wvert,x,y) = 3;
      else imRef(wvert,x,y) = 8;
    }
  }
}
 
void image2outMat(int width, int height, vlib::image<double>* input, double *out) {
    for (int x=0; x < width; x++) {
        for (int y=0; y < height; y++) {
            out[y*width+x] = imRef(input, y, x);
        }
    }
}

void mexFunction( int nlhs, mxArray *plhs[], int nrhs, const mxArray *prhs[]) {
    // test args
    /* check for proper number of arguments */
    if(nrhs!=1) {
        mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nrhs", "One inputs required.");
    }
    if(nlhs!=2) {
        mexErrMsgIdAndTxt("MyToolbox:arrayProduct:nlhs",  "Two output required.");
    }
    
    /* make sure the input argument is image */
    if( !mxIsDouble(prhs[0])) {
        mexErrMsgIdAndTxt("MyToolbox:arrayProduct:prhs", "Input must be uint8 matrix.");
    }
    
    // convert input to image
    int width = mxGetM (prhs[0]);
    int height = mxGetN (prhs[0]);
    vlib::image<vlib::uchar> *input = new vlib::image<vlib::uchar>(width, height);
    
    // insert data to image objec (equivalent to loadPGM()  )
    double *inMatrix;
    inMatrix = mxGetPr(prhs[0]);
    for (int x=0; x < width; x++) {
       for (int y=0; y < height; y++) {
           imRef(input, y, x) = inMatrix[y*width+x];
        }
    }
    
    // calling the tired labling logic
    vlib::image<double> *whorz, *wvert;
    initializeW(input, whorz, wvert);
  
    /* create the output matrix */
    plhs[0] = mxCreateDoubleMatrix(width,height,mxREAL);
    plhs[1] = mxCreateDoubleMatrix(width,height,mxREAL);

    /* get a pointer to the real data in the output matrix */
    double *out0 = mxGetPr(plhs[0]);
    image2outMat(width, height,whorz,out0);
 
    double *out1 = mxGetPr(plhs[1]);
    image2outMat(width, height, wvert, out1);
    // populate matrix

    delete whorz;
    delete wvert;
}